#include "ReconstructProcess.h"
#include <qgl.h>
#include <glm/gtx/transform.hpp>
#include "Registration.h"
#include "SingleReconstruct.h"

using namespace std;

#define FACE_THRESH 1.0

ReconstructProcess::ReconstructProcess ()
{
}

ReconstructProcess::~ReconstructProcess ()
{
	for (size_t i = 0; i < _sensors.size (); ++i)
		delete _sensors[i];
	for (size_t i = 0; i < _textures.size (); ++i)
		delete _textures[i];
}

void ReconstructProcess::addSensor (SensorDataInput *pSensor)
{
	_sensors.push_back (pSensor);
	_calibratorPtrs.push_back (new PatternCalibrationProcess (pSensor));
// 	_addModel ();
}
const SensorDataInput *ReconstructProcess::getSensor (size_t index) const
{
	if (index >= _sensors.size ())
		return NULL;
	return _sensors[index];
}
SensorDataInput *ReconstructProcess::getSensor (size_t index)
{
	if (index >= _sensors.size ())
		return NULL;
	return _sensors[index];
}

bool ReconstructProcess::grabNewFrameAndProcess ()
{
	_verticesClouds.resize (_sensors.size ());
	_faces.resize (_sensors.size ());
	SingleReconstruct singleReconstruct;
	for (int i = 0; i < _sensors.size (); ++i) {
		if (!_sensors[i]->getNewFrame ())
			return false;
 		_sensors[i]->showData ("Preview - ");
		_calibratorPtrs[i]->process ();
		singleReconstruct.setSensor (_sensors[i]);
		singleReconstruct.generateModel (500, 3000, _verticesClouds[i], _faces[i]);
	}
// 	registration ();
// 	generateModels ();
//	generateTextures ();

	return true;
}

// void ReconstructProcess::generateModels ()
// {
// 	for (size_t index = 0; index < _sensors.size (); ++index) {
// 		int frameWidth = _sensors[index]->getSensorParameter ().frameWidth;
// 		int frameHeight = _sensors[index]->getSensorParameter ().frameHeight;
// 		Model *curModel = _models[index]->getModel ();
// 		curModel->clearFace ();
// 		const QVector< glm::vec3 > &vertices = curModel->getVertices ();
// 		for (unsigned int x = 0, i = 0; x < frameWidth; ++x)
// 			for (unsigned int y = 0; y < frameHeight; ++y, ++i) {
// 				glm::vec3 vertex = _sensors[index]->get3DPoint (x, y);
// 				glm::vec2 texCoord = glm::vec2 ((float)x / (frameWidth - 1),
// 						1.0f - (float)y / (frameHeight - 1));
// 				curModel->setVertex (i, vertex);
// 				curModel->setTexCoord (i, texCoord);
// 				if (x >= 1 && y >= 1) {
// 					unsigned int corners[] = {
// 						i - frameHeight - 1, i - 1, i, i - frameHeight
// 					};
// 					if (abs (vertices[corners[0]].z - vertices[corners[3]].z) < FACE_THRESH &&
// 							abs (vertices[corners[3]].z - vertices[corners[1]].z) < FACE_THRESH &&
// 							abs (vertices[corners[1]].z - vertices[corners[0]].z) < FACE_THRESH)
// 						curModel->addFace (glm::uvec3 (corners[0], corners[3], corners[1]));
// 					if (abs (vertices[corners[1]].z - vertices[corners[3]].z) < FACE_THRESH &&
// 							abs (vertices[corners[3]].z - vertices[corners[2]].z) < FACE_THRESH &&
// 							abs (vertices[corners[2]].z - vertices[corners[1]].z) < FACE_THRESH)
// 						curModel->addFace (glm::uvec3 (corners[1], corners[3], corners[2]));
// 				}
// 			}
// 		curModel->update ();
// 	}
// }

// void ReconstructProcess::generateTextures ()
// {
// // 	cvCvtColor (rgbImage, rgbImage, CV_BGR2HSV);
// // 	for (int v = 0; v < _texture.height (); ++v) {
// // 		for (int u = 0; u < _texture.width (); ++u) {
// // 			CV_IMAGE_ELEM (rgbImage, unsigned char, v, u * 3 + 2) = 255;
// // 		}
// // 	}
// // 	cvCvtColor (rgbImage, rgbImage, CV_HSV2BGR);
// 	for (size_t i = 0; i < _sensors.size (); ++i) {
// 		IplImage *rgbImage = _sensors[i]->getRGBImage ();
// 		if (!rgbImage) continue;
// 		for (int v = 0; v < rgbImage->height; ++v) {
// 			for (int u = 0; u < rgbImage->width; ++u) {
// 				_textures[i]->setPixel (QPoint (u, v), qRgb (
// 						CV_IMAGE_ELEM (rgbImage, unsigned char, v, u * 3 + 2),
// 						CV_IMAGE_ELEM (rgbImage, unsigned char, v, u * 3 + 1),
// 						CV_IMAGE_ELEM (rgbImage, unsigned char, v, u * 3 + 0)));
// 			}
// 		}
// 	}
// }

// void ReconstructProcess::_addModel ()
// {
// 	SensorDataInput *sensor = _sensors[_sensors.size () - 1];
// 	int frameWidth = sensor->getSensorParameter ().frameWidth;
// 	int frameHeight = sensor->getSensorParameter ().frameHeight;
// 
// 	Model *modelMesh = new Model (frameWidth * frameHeight);
// 	ModelObject *newModel = new ModelObject (modelMesh);
// 	newModel->setName (QString::fromStdString (sensor->getName () + " Model"));
// 	_models.push_back (newModel);
// 
// 	QImage *texture = new QImage (640, 480, QImage::Format_RGB888);
// 	texture->fill (QColor (255, 255, 255));
// 	_textures.push_back (texture);
// }

size_t ReconstructProcess::getSensorsCount () const
{
	return _sensors.size ();
}

// std::vector< ModelObject * > & ReconstructProcess::getModels ()
// {
// 	return _models;
// }
// 
// const std::vector< ModelObject * > & ReconstructProcess::getModels () const
// {
// 	return _models;
// }

std::vector< QImage * > & ReconstructProcess::getTextures ()
{
	return _textures;
}

const std::vector< QImage * > & ReconstructProcess::getTextures () const
{
	return _textures;
}

void ReconstructProcess::registration ()
{
	std::vector< int > p0Near1Indices;
	std::vector< int > p1Near0Indices;
	_overlapClouds.resize (2);
	Registration ().registration (_verticesClouds[0], _verticesClouds[1],
			_verticesClouds[0], p0Near1Indices, p1Near0Indices, _overlapClouds[0],
			_overlapClouds[1]);
// 	glm::vec3 p0Center, p1Center;
// 	glm::vec3 p0Normal, p1Normal;
// // 	_overlapClouds[0].clear ();
// 	for (size_t i = 0; i < p0Near1Indices.size (); ++i) {
// 		p0Center += _pointClouds[0][p0Near1Indices[i]];
// 		p0Normal += _normals[0][p0Near1Indices[i]];
// // 		_overlapClouds[0].push_back (_pointClouds[0][p0Near1Indices[i]]);
// 	}
// 	p0Center /= p0Near1Indices.size ();
// 	p0Normal /= p0Near1Indices.size ();
// // 	_overlapClouds[1].clear ();
// 	for (size_t i = 0; i < p1Near0Indices.size (); ++i) {
// 		p1Center += _pointClouds[1][p1Near0Indices[i]];
// 		p1Normal += _normals[1][p1Near0Indices[i]];
// // 		_overlapClouds[1].push_back (_pointClouds[1][p1Near0Indices[i]]);
// 	}
// 	p1Center /= p1Near0Indices.size ();
// 	p1Normal /= p1Near0Indices.size ();
// 	std::wstringstream sout;
// 	sout << "PointCloud 0 Count: " << _overlapClouds[0].size () << std::endl;
// 	sout << "PointCloud 0 Center:" << p0Center.x << ", " << p0Center.y << ", "
// 			<< p0Center.z << std::endl;
// 	sout << "PointCloud 0 Normal:" << p0Normal.x << ", " << p0Normal.y << ", "
// 			<< p0Normal.z << std::endl;
// 	for (size_t i = 0; i < 5; ++i) {
// 		for (size_t j = 0; j < 5; ++j) {
// 			sout << _overlapClouds[0][i * 5 + j].x << ", "
// 					<< _overlapClouds[0][i * 5 + j].y << ", "
// 					<< _overlapClouds[0][i * 5 + j].z << "\t";
// 		}
// 		sout << std::endl;
// 	}
// 	sout << "PointCloud 1 Count: " << _overlapClouds[1].size () << std::endl;
// 	sout << "PointCloud 1 Center:" << p1Center.x << ", " << p1Center.y << ", "
// 			<< p1Center.z << std::endl;
// 	sout << "PointCloud 1 Normal:" << p1Normal.x << ", " << p1Normal.y << ", "
// 			<< p1Normal.z << std::endl;
// 	for (size_t i = 0; i < 5; ++i) {
// 		for (size_t j = 0; j < 5; ++j) {
// 			sout << _overlapClouds[1][i * 5 + j].x << ", "
// 				<< _overlapClouds[1][i * 5 + j].y << ", "
// 				<< _overlapClouds[1][i * 5 + j].z << "\t";
// 		}
// 		sout << std::endl;
// 	}
// 	OutputDebugString (sout.str ().c_str ());

	//glm::mat4 finalTransform = glm::translate (p1Center - p0Center);
	//for (size_t i = 0; i < _pointClouds[0].size (); ++i) {
	//	_pointClouds[0][i] = (finalTransform * glm::vec4 (_pointClouds[0][i], 1.0)).
	//			swizzle (glm::X, glm::Y, glm::Z);
	//}
}

std::vector< std::vector< Vertex > > & ReconstructProcess::getOverlapClouds ()
{
	return _overlapClouds;
}

std::vector< std::vector< Vertex > > & ReconstructProcess::getVerticesClouds ()
{
	return _verticesClouds;
}

const std::vector< std::vector< Vertex > > & ReconstructProcess::getVerticesClouds () const
{
	return _verticesClouds;
}
